#ifndef WORKSPACE_DEPTH_CAMERA_ROS_DRIVER_H
#define WORKSPACE_DEPTH_CAMERA_ROS_DRIVER_H

#include <string>
#include <filesystem>
#include <iostream>
#include <unistd.h>
#include <sys/time.h>
#include <iostream>
#include <csignal>
#include <thread>

#include <sys/stat.h>

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/calib3d.hpp>

#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <cv_bridge/cv_bridge.h>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

#include "sdk/api.h"
#include "sdk/picojson.h"

enum RGBD_MODE
{
    RGBD_CLOSE=0,
    DEPTH2RGB_RESIZE,
    DEPTH2RGB_FILTER,
    RGB2DEPTH,

};

struct StrcParam{
    // TOF相机参数
    int Width_tof = 0;
    int Height_tof = 0;
    double fx_tof;
    double fy_tof;
    double cx_tof;
    double cy_tof;
    double k1_tof;
    double k2_tof;
    double k3_tof;
    double p1_tof;
    double p2_tof;

    // RGB相机参数
    int Width_rgb = 0;
    int Height_rgb = 0;
    double fx_rgb;
    double fy_rgb;
    double cx_rgb;
    double cy_rgb;
    double k1_rgb;
    double k2_rgb;
    double k3_rgb;
    double p1_rgb;
    double p2_rgb;

    // RGB-D参数
    int Width_rgbd = 0;
    int Height_rgbd = 0;
    int Width_rgbd_cut = 0;
    double fx_rgbd;
    double fy_rgbd;
    double cx_rgbd;
    double cy_rgbd;

    // 外参
    double R[3][3];  // 旋转矩阵
    double T[3];      // 平移向量

    Eigen::Matrix3f R_;
    Eigen::Vector3f T_; 
};

class EdsRosDriver {
public:
    EdsRosDriver();
    ~EdsRosDriver(){};

    void run();

private:
    bool isRunningOnOrin();
    void myResize(const cv::Mat& src, cv::Mat& dst, const cv::Size& size);
    void depthToPointCloud(const cv::Mat &depth, 
        pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, bool is_rgb_camera);
    
    void convertDepthToPointCloud2(cv::Mat &depth, sensor_msgs::PointCloud2 &cloud_msg, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
    void undistortImage(const cv::Mat &src, cv::Mat &dst, bool is_rgb);
    int parseIntrinsicParameters(const char* pInfo);
    void alignDepthToRGB_Resize( 
        cv::Mat &depth_aligned, pcl::PointCloud<pcl::PointXYZ>::Ptr &rgb_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &tof_cloud);
    void alignRGBToDepth( 
        std::vector<unsigned char> &rgb_points, 
        cv::Mat undistorted_rgb, pcl::PointCloud<pcl::PointXYZ>::Ptr &rgb_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &tof_cloud);
    void alignDepthToRGB_Filter(
        cv::Mat &depth_aligned, cv::Mat undistorted_rgb, pcl::PointCloud<pcl::PointXYZ>::Ptr &rgb_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &tof_cloud);        
    void convertAlignedDepthToPointCloud2(const cv::Mat &aligned_depth, sensor_msgs::PointCloud2 &cloud_msg);
    void bilateralFilterWithGuide(const cv::Mat& depth, const cv::Mat& guide, cv::Mat& filtered_depth,
                                  int kernel_size, float sigma_space, float sigma_color);
    
    void computeUndistortMaps(const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs, const cv::Size& imageSize, cv::Mat& map1, cv::Mat& map2);

    ros::NodeHandle nh_;
    ros::NodeHandle pn_;

    ros::Publisher depth_pub_;
    ros::Publisher ir_pub_;
    ros::Publisher rgb_pub_;
    ros::Publisher cloud_pub_;
    ros::Publisher rgbd_depth_pub_;
    ros::Publisher rgbd_rgb_pub_;
    ros::Publisher rgbd_depth_cloud_pub_;

    int handle_ = -1;

    bool isRunningOnOrin_;
    bool rgb_open_;
    bool cloud_open_;
    std::string ip_;
    int exposure_;
    int resolution_height_;
    bool cut_flag_;
    int rgbd_mode_;

    int depth_height_;
    int depth_width_;
    int rgb_height_;
    int rgb_width_;

    float fx_;
    float fy_;
    float cx_;
    float cy_;
    float k1_;
    float k2_;
    float k3_;
    float p1_;
    float p2_;

    float rgb_fx_;
    float rgb_fy_;
    float rgb_cx_;
    float rgb_cy_;
    float rgb_k1_;
    float rgb_k2_;
    float rgb_p1_;
    float rgb_p2_;
    float rgb_k3_;

    cv::Mat TOF_camera_matrix = cv::Mat::eye(3, 3, CV_32F);
    cv::Mat RGB_camera_matrix = cv::Mat::eye(3, 3, CV_32F);

    cv::Mat TOF_dist_coeffs = cv::Mat::zeros(1, 5, CV_32F);
    cv::Mat RGB_dist_coeffs = cv::Mat::zeros(1, 5, CV_32F);

    cv::Mat TOF_Undistort_Map_1, TOF_Undistort_Map_2;
    cv::Mat RGB_Undistort_Map_1, RGB_Undistort_Map_2;

    bool init_flag_;

    int param_version_;

    StrcParam mParam;
};

#endif
